#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include "hins/xingsong_driver.h"

using namespace hins;

sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_angle, const float& end_angle, const std::string& frame_name);
void ParamInit(ros::NodeHandle &nh_private ,ConnectionAddress &laser_conn_info, ShadowsFilterParam &shadows_filter_param);

int main(int argc, char** argv){

  // 1. ROS 初始化
  ros::init(argc, argv, "hins_driver_laser");
  ros::NodeHandle nh_;
  ros::NodeHandle nh_private("~");

  float start_angle, end_angle;
  std::string topic_name,frame_name;
  nh_private.param<float>("start_angle", start_angle, 0.);
  nh_private.param<float>("end_angle", end_angle, 2 * M_PI);
  nh_private.param<std::string>("topic_name", topic_name, std::string("scan1"));
  nh_private.param<std::string>("frame_name", frame_name, std::string("laser1"));

  ROS_INFO("advertise: %s", topic_name.c_str());
  ros::Publisher scan_pub = nh_.advertise<sensor_msgs::LaserScan>(topic_name, 3, true);

  // 2. 雷达驱动
  ConnectionAddress laser_conn_info;
  ShadowsFilterParam shadows_filter_param;

  // 初始化驱动参数
  ParamInit(nh_private,laser_conn_info, shadows_filter_param);

  // 启动雷达
  XingSongDriverHdr driver_hdr = std::make_shared<XingSongDriver>(laser_conn_info,shadows_filter_param);

  // 3. ros 消息发布
  ros::Rate r(50);
  while(ros::ok()){
    ScanData data = driver_hdr->GetFullScan();
    sensor_msgs::LaserScanPtr scan_msg_ptr = ToLaserScan(data, start_angle, end_angle, frame_name);
    if(scan_msg_ptr->ranges.size() != 0)            // 如果数据长度有误，则不发布
    {  
      scan_pub.publish(scan_msg_ptr);
    }
    r.sleep();
  }
}

sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_angle, const float& end_angle, const std::string& frame_name)
{
  sensor_msgs::LaserScanPtr ret = boost::make_shared<sensor_msgs::LaserScan>();

  ret->header.frame_id = frame_name;
  ret->header.stamp = ros::Time::now();

  ret->range_min = 0.05;
  ret->range_max = 10.0;
  ret->angle_min = M_PI/2.0f;
  ret->angle_max = 3.0 * M_PI/2.0f;

  ret->time_increment = 1/20/float(data.distance_data.size());        // 一个周期时间除以扫描点数

  ret->angle_increment = 1.5 * M_PI/float(data.distance_data.size());

  ret->ranges.resize(data.distance_data.size(), 0);
  ret->intensities.resize(data.amplitude_data.size(), 0);
  
  for( std::size_t i = 0; i < data.distance_data.size(); i++ )
  {
    float angle = ret->angle_min + i * ret->angle_increment;
  
    float dis = float(data.distance_data[i])/1000.0f;
    
    if(angle < start_angle || angle > end_angle)
      ret->ranges[i]  = 1024.0;
    else
      ret->ranges[i] = dis;
    ret->intensities[i] = data.amplitude_data[i];
  }

  return ret;
}


void ParamInit(ros::NodeHandle &nh_private ,ConnectionAddress &laser_conn_info, ShadowsFilterParam &shadows_filter_param)
{
  std::string server_address;
  float start_angle, end_angle, shadows_filter_max_angle, shadows_filter_min_angle;
  int port, shadows_filter_neighbors, shadows_filter_window, shadows_filter_level, shadows_filter_traverse_step;

  nh_private.param("laser_ip", server_address, std::string("192.168.1.88"));
  nh_private.param<int>("laser_port", port, 8080);
  nh_private.param<float>("shadows_filter_max_angle", shadows_filter_max_angle, float(175.0));
  nh_private.param<float>("shadows_filter_min_angle", shadows_filter_min_angle, float(5.0));
  nh_private.param<int>("shadows_filter_neighbors", shadows_filter_neighbors, 1);
  nh_private.param<int>("shadows_filter_window", shadows_filter_window, 2);
  nh_private.param<int>("shadows_filter_traverse_step", shadows_filter_traverse_step, 1);
  nh_private.param<int>("shadows_filter_level", shadows_filter_level, -1);                           // 默认按照具体参数配置

  laser_conn_info.SetAddress(server_address);
  laser_conn_info.SetPort(port);

  shadows_filter_param.max_angle = shadows_filter_max_angle;
  shadows_filter_param.min_angle = shadows_filter_min_angle;
  shadows_filter_param.neighbors = shadows_filter_neighbors;
  shadows_filter_param.window = shadows_filter_window;
  shadows_filter_param.traverse_step = shadows_filter_traverse_step;
  shadows_filter_param.shadows_filter_level = shadows_filter_level;
}